
//需要用到之前的spline库

// 算法解析：
// 这是用到了一个纯P控制器去控制速度，还是用到了一个预瞄点去控制车辆的转角
// 2020/7/14


void Pure_pursuit::createControlCommad() {
  // steering control

  fsd::Vec_f wx, wy;
  for(auto &iter: path_) {
    wx.push_back(iter.pose.position.x);
    wy.push_back(iter.pose.position.y);
  }
  fsd::Spline2D splinePath(wx,wy);


  std::array<float, 2> point_ = splinePath.calc_postion(look_distance_);
  next_point_.y = point_[1];
  next_point_.x = point_[0];
  std::cout<<"x: "<<next_point_.x<<"\ny: "<<next_point_.y<<std::endl;

  const double ld = std::hypot(next_point_.y,
                               next_point_.x);
  const double alpha = std::atan2(next_point_.y,
                                  next_point_.x);
  // steering control
  cmd_.steering_angle.data =
      static_cast<float>(std::atan(2 * car_length_ * std::sin(alpha) / ld));
  std::cout << "steering_angle: " << cmd_.steering_angle.data << std::endl;

  double sample_time = 0.5;
  double vel = std::hypot(state_.car_state_dt.car_state_dt.x, state_.car_state_dt.car_state_dt.y);
  double a = std::hypot(state_.car_state_dt.car_state_a.x, state_.car_state_dt.car_state_a.y);
  double ref_distance =  vel * sample_time + 0.5 * a * sample_time * sample_time;
  if(ref_distance > 8) ref_distance = 8;
  float curva = splinePath.calc_curvature(ref_distance);
  std::cout<<"vel: "<<vel<<std::endl;
  std::cout<<"ref_distance: "<<ref_distance<<std::endl;
  std::cout<<"curvature: "<<curva<<std::endl;
  double target_v = max_speed_ - 60 * fabs(curva);
  // speed control
  cmd_.throttle.data = static_cast<float>(speed_p_ * (target_v - vel));
}

